#include "message.h"

#include <stdio.h>

Message::Message()
{
    // find real endian type
	/*
    unsigned char EndianTest[2] = {1,0};
    short x = *(short *)EndianTest;

    if( x == 1 ) endianType = P_BIGENDIAN;
    else endianType = P_LITTLEENDIAN;
	*/

	short int test = 0x0001;
	char *byte = (char*)&test;

	if( byte[0] == 0x01 )
		endianType = P_LITTLEENDIAN;
	else
		endianType = P_BIGENDIAN;


    holder = "";
}

void Message::convertEndian(void *inPtr, void *outPtr, long size)
{
	char *ptr1 = (char*)inPtr;
	char *ptr2 = (char*)outPtr;

	int boundSize = size - 1;

	for(int i = 0; i < size; i++)
	{
		ptr2[i] = ptr1[boundSize - i];
	}
}

/* Converts an integer to Big Endian.
   If already Big Endian, reverts it
   to native format */
long Message::iFixEndian(long val)
{
	if( endianType == P_BIGENDIAN )
	{
		long out;
		convertEndian(&val, &out, sizeof(long));
		return out;
	}
	else
		return val;

	/*
    unsigned char b1, b2, b3, b4;
    b1 = val & 255;
    b2 = ( val >> 8 ) & 255;
    b3 = ( val >> 16 ) & 255;
    b4 = ( val >> 24 ) & 255;
    return ((int)b1 << 24) + ((int)b2 << 16) + ((int)b3 << 8) + b4;
	*/
}

/* Converts a double to Big Endian.
   If already Big Endian, reverts it
   to native format */
double Message::fFixEndian(double val)
{
	if( endianType == P_BIGENDIAN )
	{
		double out;
		convertEndian(&val, &out, sizeof(double));
		return out;
	}
	else
		return val;

	/*
    unsigned char *pbytes;
    pbytes = (unsigned char *)&val;

    // swap values
    unsigned char tmp = pbytes[7];
    pbytes[7] = pbytes[0];
    pbytes[0] = tmp;
    tmp = pbytes[6];
    pbytes[6] = pbytes[1];
    pbytes[1] = tmp;
    tmp = pbytes[5];
    pbytes[5] = pbytes[2];
    pbytes[2] = tmp;
    tmp = pbytes[4];
    pbytes[4] = pbytes[3];
    pbytes[3] = tmp;

    return val;
	*/
}

void Message::push_string(char *instr, unsigned int len)
{
    // get the length of the string (in big endian)
	len = iFixEndian(len);

    // push data type 's'
    holder.push_back('s');

    // convert length to bytes
    // push them
    //unsigned char bytes[sizeof(long)];
    unsigned char *pbytes = (unsigned char *)&len;
    for(unsigned int i = 0; i < sizeof(long); i++)
        holder.push_back(pbytes[i]);

    // and finally, the actual string.
    holder.append(instr, len);
}

void Message::push_string(std::string instr)
{
    // get the length of the string (in big endian)
    unsigned long len = instr.length();
	len = iFixEndian(len);

    // push data type 's'
    holder.push_back('s');

    // convert length to bytes
    // push them
    //unsigned char bytes[sizeof(long)];
    unsigned char *pbytes = (unsigned char *)&len;
    for(unsigned int i = 0; i < sizeof(long); i++)
    holder.push_back(pbytes[i]);

    // and finally, the actual string.
    holder.append(instr);
}

void Message::push_integer(long inlong)
{
    // convert to big endian
	inlong = iFixEndian(inlong);


    // convert to bytes
    //unsigned char bytes[sizeof(long)];
    unsigned char *pbytes = (unsigned char *)&inlong;

    // data type i
    holder.push_back('i');
    for(unsigned int i = 0; i < sizeof(long); i++)
        holder.push_back(pbytes[i]);
}

void Message::push_float(double indouble)
{
    // convert to big endian
	indouble = fFixEndian(indouble);

    // convert to bytes
    //unsigned char bytes[sizeof(double)];
    unsigned char *pbytes = (unsigned char *)&indouble;

    // data type F
    holder.push_back('f');
    for(unsigned int i = 0; i < sizeof(double); i++)
        holder.push_back(pbytes[i]);
}


// peeks at the next type in the packet for reading
// returns P_TYPE_ERROR if packet is empty
// returns P_TYPE_ERROR if type cannot be read
// otherwise, returns P_TYPE_STRING, P_TYPE_INTEGER,
// or P_TYPE_FLOAT, respectively
int Message::get_next_type()
{
    if( holder.length() == 0 )
        return P_TYPE_ERROR;

    int ntype = holder.at(0);
    switch( ntype )
    {
        case 's': return P_TYPE_STRING; break;
        case 'i': return P_TYPE_INTEGER; break;
        case 'f': return P_TYPE_FLOAT; break;
        default: return P_TYPE_ERROR; break;
    }

    return P_TYPE_ERROR;
}


std::string Message::read_string()
{
    // make sure it's a string
    if( holder.at(0) != 's' )
        return "";

    // next 4 bytes represent length
    unsigned char bytes[sizeof(long)];
    unsigned char *pbytes = &bytes[0];
    pbytes = ((unsigned char*)&holder[1]);
    unsigned long len = *(unsigned long*)pbytes;
	len = iFixEndian(len);

    // read the actual string
    std::string retstr = holder.substr(1+sizeof(long), len);

    // remove it from the packet now
    holder.erase(0, 1 + sizeof(long) + len);

    return retstr;
}

long Message::read_integer()
{
    // make sure its an integer
    if( holder.at(0) != 'i' )
        return 0;

    // next 4 bytes represent data
    unsigned char bytes[sizeof(long)];
    unsigned char *pbytes = &bytes[0];
    pbytes = ((unsigned char*)&holder[1]);
    unsigned long data = *(unsigned long*)pbytes;
	data = iFixEndian(data);

    holder.erase(0, 1 + sizeof(long));
    return data;
}

double Message::read_float()
{
    // make sure its a float
    if( holder.at(0) != 'f' )
        return 0.0;

    // next 8 bytes represent data
    unsigned char bytes[sizeof(double)];
    unsigned char *pbytes = &bytes[0];
    for(unsigned int i = 0; i < sizeof(double); i++)
        bytes[i] = holder.at(i+1);

    double data = *(double*)pbytes;

	data = fFixEndian(data);

    holder.erase(0, 9);
    return data;
}

// returns a human readable string including all datas
// and type markers
std::string Message::get_debug_string()
{
    std::string retval = "";

    unsigned int readpos = 0;

    while( readpos < length() )
    {
        switch( holder.at(readpos) )
        {
            case 's': {
                unsigned char bytes[sizeof(long)];
                unsigned char *pbytes = &bytes[0];
                pbytes = ((unsigned char*)&holder[readpos+1]);
                unsigned long len = *(unsigned long*)pbytes;
				len = iFixEndian(len);

                // read the actual string
                std::string strval = holder.substr(readpos+5, len);

                // convert all white-space characters to spaces
                for(unsigned int i = 0; i < strval.length(); i++)
                {
                    if( strval.at(i) < 0x20 || strval.at(i) > 0x7E )
                    strval[i] = ' ';
                }

                char numbuff[32]; // convert length to string
                snprintf((char*)&numbuff, 32, "%d", (int)len);

                // dump it into string 'retval'
                retval += "string(";
                retval += (char*)&numbuff;
                retval += "):";
                retval += strval;
                retval += '\n';

                readpos = readpos + 1 + sizeof(long) + len; // update readpos
            } break;

            case 'i': {
                unsigned char bytes[sizeof(long)];
                unsigned char *pbytes = &bytes[0];
                pbytes = ((unsigned char*)&holder[readpos+1]);
                unsigned long data = *(unsigned long*)pbytes;
				data = iFixEndian(data);

                char numbuff[32]; // convert data to string
                snprintf((char*)&numbuff, 32, "%d", (int)data);

                // dump it into string 'retval'
                retval += "int:";
                retval += (char*)&numbuff;
                retval += '\n';

                readpos = readpos + 1 + sizeof(long); // update readpos
            } break;

            case 'f': {
                unsigned char bytes[sizeof(double)];
                unsigned char *pbytes = &bytes[0];
                for(unsigned int i = 0; i < sizeof(double); i++)
                bytes[i] = holder.at(readpos+i+1);

                double data = *(double*)pbytes;

				data = fFixEndian(data);

                char numbuff[65]; // convert data to string
                snprintf((char*)&numbuff, 65, "%f", data);

                // dump it into string 'retval'
                retval += "float:";
                retval += (char*)&numbuff;
                retval += '\n';

                readpos = readpos + 1 + sizeof(double); // update readpos
            } break;
        }
    }

    return retval;
}

void Message::set_data(const char *str, unsigned int len)
{
	holder.clear();
    //holder.assign(len, *str);
	for(unsigned int i = 0; i < len; i++)
	{
		holder.push_back( str[i] );
	}
}

void Message::set_data(const std::string &instr)
{
    holder.assign(instr);
}

unsigned int Message::length()
{
    return holder.length();
}

std::string Message::str()
{
	return holder;
}

const char *Message::c_str()
{
    return holder.c_str();
}
